# fake car py
#
# simulates a multi rocket car and builds packets
# see rocketwheels ocx 2011 FakeCar.h and FakeCar.cpp for origonal
#
# basic sequence (technical)
#   1) create a randomish configuration of physical properties,
#       number of rockets, number of stages, and fuse delays
#   2) assemble the rockets into a 'force span' array to approximate f(t)
#   3) forward integrate f(t) to approximate a(t) and v(t0
#   4) for each station distance x on the track, approximate t(x) using the result of step 3
#   5) using system time, emit packets for each station

import random
from collections import namedtuple

import app.RaceTrack as RaceTrack

import app.PassData as PassData
from app.PassData import PacketEvent
from app.PassData import PacketEventList

import app.Physical as Physical
import app.Receiver as Receiver


## ################################################################################################################################
## ################################################################################################################################
## ################################################################################################################################


# randomness

def apply_order( val, order ) :
    """ helper function for random """
    # clip order arg
    order = 1 if order < 1 else 8 if order > 8 else order
    ret = val
    while order > 1 :
        ret *= abs(val)
        order -= 1
    return ret

def frand_range( center, range, order ) :
    """ returns a random number close to center,
        never farther away than +- range,
        using order as a polynomial ddistribution
    """
    val = apply_order( random.uniform(-1.0,1.0), order )
    return center + val * range

def irand_range(range, order) :
    """ returns an in between 0 and range, uses order to nudge towards zero """
    val = apply_order( random.uniform(0.0,1.0), order )
    return int( val*range )




## ################################################################################################################################
## ################################################################################################################################
## ################################################################################################################################


## ForceSpan

# a Force Span is represents a trapeziodal shaped segment of force plotted against time
# we'll have a list of ForceSpan records in time ascending time order -- with gaps

ForceSpan = namedtuple( 'ForceSpan', 't0 t1 f0 f1' )

# a ForceSpanList is a list of ForceSpan records
# insert forcespan dices and slices in order to insert into the proper location

def insert_forcespan( force_span_list, f ) :
    """ given a list of ForceSpan
        insert the passed in ForceSpan f
        chopping previous records as required
    """


    # base case
    if len(force_span_list)==0 :
        #print("case A")
        return [f]



    # define a local l and ls
    l = force_span_list[0]
    ls = force_span_list[1:]
    # simple insert before
    if f.t1 <= l.t0 :
        #print("case B")
        return [f] + force_span_list
    # recursive case : simple insert after
    if f.t0 >= l.t1 :
        #print("case C")
        return [l] + insert_forcespan( ls, f )



    # the rest involve splitting and combining ranges
    def split(x,at) :
        # lerp the force
        f = ( (x.f1-x.f0) / (x.t1-x.t0) ) * (at-x.t0) + x.f0
        # build two new spans
        g = ForceSpan( t0=x.t0, t1=at,   f0=x.f0, f1=f    )
        h = ForceSpan( t0=at,   t1=x.t1, f0=f,    f1=x.f1 )
        #print("split( x={}, at={} --> ( g={}, h={} )".format( x,at,g,h ) )
        # return them
        return ( g, h )
    def combine(x,y) : # the t's had better match
        z = [ ForceSpan( t0=x.t0, t1=x.t1, f0=x.f0+y.f0, f1=x.f1+y.f1 ) ]
        #print("combine( x={}, y={} --> z={}".format( x,y,z ) )
        return z




    ##D   l          l0--------l1
    ##   +f    f0-----------         f0 < l0
    ##   =     g0---g1h0----         (g,h) = split f at l0, return [g] + recurse( [l]+ls, [h] )
    if f.t0 < l.t0 :
        #print("case D")
        (g,h) = split(f,l.t0)
        return [g] + insert_forcespan( force_span_list, h )

    ##E   l    l0--------------l1    f0>l0
    ##   +f          f0-----
    ##   =     g0--g1h0--------h1    (g,h) = split(l) at f0, return [g] + recurse( [h]+ls, f  )
    if f.t0 > l.t0 :
        #print("case E")
        (g,h) = split(l,f.t0)
        return [g] + insert_forcespan( [h]+ls, f )

    # f0==l0
    ##F  l     l0-------------l1
    ##  +f     f0-------f1           f0==l0 and f1<l1
    ##  =      g0-------g1h0--h1     (g,h) = split(l) at f1, return [f+g] + [h] + ls
    if f.t1 < l.t1 :
        #print("case F")
        ( g, h ) = split( l,f.t1 )
        return combine(f,g) + [h] + ls

    ##G  l     l0-------l1
    ##  +f     f0-------------f1     f0==l0 and f1>l1
    ##  =      g0-------g1h0--h1     (g,h) = split(f) at l1, return [l+g] + recurse( ls, h )
    if f.t1 > l.t1 :
        #print("case G")
        (g,h) = split(f,l.t1)
        return combine(l,g) + insert_forcespan( ls, h )

    ##H   l    l0--------l1
    ##   +f    f0--------f1          f0==l0 and f1 == l1
    ##   =     t0--------t1          return [f+l] + ls
    #if f.t1==l.t1 :
    #print("case H")
    return combine( f,l ) + ls




#   def test_insert_forcespan() :
#       fsl = []
#
#       f = ForceSpan( 1,3,10,0 )
#       print("\ninsert {}".format(f))
#       fsl = insert_forcespan( fsl, f )
#       print(fsl)
#
#       f = ForceSpan( 1,3,10,0 )
#       print("\ninsert {}".format(f))
#       fsl = insert_forcespan( fsl, f )
#       print(fsl)
#
#       f = ForceSpan( 5,7,10,0 )
#       print("\ninsert {}".format(f))
#       fsl = insert_forcespan( fsl, f )
#       print(fsl)
#
#       f = ForceSpan( 3,5,10,0 )
#       print("\ninsert {}".format(f))
#       fsl = insert_forcespan( fsl, f )
#       print(fsl)
#
#       f = ForceSpan( 0.5,2,10,0 )
#       print("\ninsert {}".format(f))
#       fsl = insert_forcespan( fsl, f )
#       print(fsl)
#
#       f = ForceSpan( 6,8,10,0 )
#       print("\ninsert {}".format(f))
#       fsl = insert_forcespan( fsl, f )
#       print(fsl)
#
#       return fsl
#
#   #   test_insert_forcespan()


## ################################################################################################################################
## ################################################################################################################################
## ################################################################################################################################

## LocationSpan

## after a force span list is constructed
## friction and mass must be applied
## from there we can forward integrate
## to determine velocity and distance
## which results in a LocationSpan


LocationSpan = namedtuple( 'LocationSpan', 't0 t1 a0 a1 v0 v1 x0 x1' )

def forward_integrate( force_span_list, mass, friction ) :
    """ transforms a list of ForceSpan record into a list of LocationSpan records
        fills in gaps
    """

    # return value is a list of LocationSpan
    ret = []

    # runing state
    t0 = 0
    v0 = 0
    x0 = 0

    # local helpers
    def do_integrate_and_advance(fs) :
        nonlocal t0,v0,x0
        t1 = fs.t1
        # do the dumb check first -- if the car is stopped and there is no force
        if v0==0 and fs.f0==0 and fs.f1==0 :
            a0 = 0
            a1 = 0
            v1 = v0
            x1 = x0
        # the full case -- see integrate.mcd
        else :
            dt = fs.t1-fs.t0
            a0 = fs.f0/mass - friction
            a1 = fs.f1/mass - friction
            v1 = v0 + dt * (a1+a0) / 2.0
            if v1<0 : # car stopped, figure out where
                new_dt = -2.0*v0/(a0+a1)
                new_a1 = a0 + (a1-a0) * new_dt / dt
                dt = new_dt
                a1 = new_a1
                v1 = 0
                t1 = t0 + new_dt
            #determine the end location
            x1 = ( 2.0*a0 + a1 ) * dt * dt / 6.0 + v0 * dt + x0

        # now build and store new LocationSpan record
        gs = LocationSpan( t0=t0, t1=t1, a0=a0, a1=a1, v0=v0, v1=v1, x0=x0, x1=x1 )
        ret.append( gs )
        # and advance the outer counters
        t0=t1
        v0=v1
        x0=x1

    # to work, scan the list
    for fs in force_span_list :

        # make sure we fill gaps
        while t0 < fs.t0 :
            # create a zero force span (will have only friction)
            gs = ForceSpan( t0=t0, t1=fs.t0, f0=0, f1=0 )
            # integrate it
            do_integrate_and_advance(gs)

        # now do the fs record
        do_integrate_and_advance(fs)

    # now all the forces are gone, but the car might still be moving
    # so we add a final span
    gs = ForceSpan( t0=t0, t1=100000000, f0=0, f1=0 )
    do_integrate_and_advance(gs)

    # and that should do it
    return ret



#   # test forward integrate
#
#   fsl = test_insert_forcespan()
#   lsl = forward_integrate( fsl, 10, 0.1 )
#   print()
#   print()
#   print(lsl)

## ################################################################################################################################
## ################################################################################################################################
## ################################################################################################################################

## once we have a list of location span
## we can querry when it will be at a particular location
## this is done here -- and returns None if the car can never get that far

def get_time_of_position(location_span_list, position) :

    # first, find the frame ls with this record
    for ls in location_span_list :
        if position <  ls.x0 : return None # there's no gaps, this is un necessary
        if position == ls.x0 : return ls.t0
        if position <  ls.x1 : break
        if position == ls.x1 : return ls.t1
    # iteration ends on the last record
    # so check (again!) and bail if it's not there
    if position > ls.x1 :
        return None

    # since x(t) is a 3rd order polynomial, and difficult to solve,
    # we'll do a stupid bin squeeze search
    k3 = (ls.a1-ls.a0) / ( 6.0 * (ls.t1-ls.t0) )
    k2 = ls.a0 / 2.0
    k1 = ls.v0
    k0 = ls.x0
    iota = 2.5e-4 # about the smallest we can do before round off makes us get stuck
    # start at full span
    t0 = 0
    t1 = ls.t1 - ls.t0
    # do the squeeze
    #print( "finding {} in {}".format(position,ls) )
    count = 0 # just in case, 30 counts is sufficient ... would be better to monitor the value change ... too lazy
    while True :
        dw = (t1+t0) * 0.5
        x = dw * ( dw * ( dw*k3 + k2 ) + k1 ) + k0
        #print( "  t0={:3.3f} dw={:3.3f} t1={:3.3f} x={:3.3f}".format(t0,dw,t1,x) )
        count += 1
        if count > 30 :
            #print( "STUCK!!" )
            return ls.t0 + dw
        if   position < x-iota : t1 = dw # squeeze to the left
        elif position > x+iota : t0 = dw # squeeze to the right
        # otherwise we found it
        else : return ls.t0 + dw


#   # test it
#   print()
#   print()
#
#   x = 0
#   t = 0
#   while t is not None :
#       t = get_time_of_position(lsl, x)
#       print( x,t )
#       x += 1



## ################################################################################################################################
## ################################################################################################################################
## ################################################################################################################################

## with a LocationSpanList we can create an EventList
## it will state when packets are to be fired
## to build a PacketEventList we need a list of StationDistance records
## which we get from the race_tack


def build_packet_event_list( race_track, location_span_list, distance_offset, initial_network_clock ) :
    """ merges a list of LocationSpan records and RaceTrack.track_distance into PacketEvents
        distance_offset is the distance the car is placed before the first sensor
        initial_network_clock is used to set the packet time field
        returns a PacketEventList
    """

    # cache this here to add to the net clck for each event
    t_clk = Physical.convert( (initial_network_clock,race_track.tick_rate), 'tick,rate' )

    # start with an empty list and a function to add to that list
    pel = [] # packet event list --
        #PacketEvent = namedtuple( 'PacketEvent',
        #    [   'timestamp',    # float -- system timestamp either when recieved or when to sent
        #        'packet'        # string -- data packet from pysical track
        #    ] )

    # do the work
    acc_x = 0 # x0 accumulator
    for (s,sd) in enumerate( race_track.station_distance_list ) :
        # s is station number
        # sd is station data
        acc_x += sd[0].nom # add the distance from the prev station
        acc_dx = 0 # dx accumulator for this station
        for ( c,d ) in enumerate(sd) :
            # c is the sensor number
            # d is the distance to previous sensor -- [0] is special
            if c : acc_dx += d.nom
            ## try to add the event
            t = get_time_of_position(location_span_list, acc_x + acc_dx + distance_offset )
            if t is None : return PacketEventList(pel)

            #b = (1<<c) + ((1<<c)-1) # bit code
            ### add the event
            ##t is in seconds         t must be converted to network clocks
            ##s is station number     b is sensor code bits
            ##we'd like to account for lockout time ...       but won't ?

            #e = Physical.convert( ( t + t_clk, race_track.tick_rate ), 'sec','tick,rate' )
            #pkt = "{}{}{:08X}".format(s,b,e)

            # here we need to go through the proper encoder
            pkt = Receiver.encode_packet(race_track,s,c, t+t_clk )

            pel.append( PacketEvent( timestamp=t, packet=pkt ) )
            ## advance the accumulaters

        # we should do something the c codes, they could turn off

    return PacketEventList(pel)





## ################################################################################################################################
## ################################################################################################################################
## ################################################################################################################################

## we now have enough machinery in place to create random rocket configurations
## these have can have multiple stages and rockets per stages
## when a rocket is built, we can define its LocationSpanList
## which would be passed along to the packet generator
## ultimately returning the desired PassData


def create_fake_car(race_track,user_mass=None) :
    """ creates a PassData struct to be sent to a RaceTrack
        race_track    A RaceTrack that the car will be run on
        user_mass     set to a number in kg or leave blank
    """

   	# specs are center+/-deviation and order

   	# mass (gm) 40+/-30, quadratic
    # in MKS we get  0.04 +/- 0.03, linear
    if user_mass is not None :
        # user val +/- 10% with quadratic distribution
        mass = frand_range( user_mass, user_mass*0.1, 2 )
    else :
        # otherwise, linear over a reasonable range
        mass = frand_range(0.040, 0.035, 1)

    # friction (gmf) 0.3+/-0.1, linear
    friction = frand_range( 0.3, 0.1, 2 ) # not sure on units ...

    # stages ranges from 1 to 3 with 50%,40%,10% probability
    #                                0.5     0.9
    ftemp = frand_range(0.5,0.5,1)
    if   ftemp <= 0.5 : num_stages = 1
    elif ftemp <= 0.9 : num_stages = 2
    else :              num_stages = 3

	# fuse length between stages (measured in second) is 1.75+/-1.25, quadratic (0.5 to 3)
	# we accumulate all into stage start times

    # first stage is longer -- start of race -- but we make it short for dev
    #stage_start_time = [ frand_range( 7.0, 5.0, 1 ) ] # 2 to 12 seconds just to start
    stage_start_time = [ frand_range( 2.0, 1.0, 1 ) ] # 1 to 3 seconds just to start
    # next stages accumulate the previous
    for i in range( 1,3 ) : # =1 ; i<num_stages ; i++ )
        stage_start_time.append( stage_start_time[-1] + frand_range( 1.75, 1.25, 2 ) )

	# total number of rockets is num_stages plus 0 to 18, quadratic
    num_rockets = num_stages + irand_range( 18, 2 )

	# rockets assigned arbitrarily to stages with equal probability
    # make sure there is at least 1 in each
    stage_rockets = dict() # use empty dict
    for i in range( 0, num_rockets ) :
        stage = i if i<num_stages else irand_range(num_stages, 1 )
        if stage not in stage_rockets :
            stage_rockets[stage] = 0
        stage_rockets[stage] += 1


    # add force spans for all the rockets
	# apply a fuse length adjust per rocket (added to interstage length) is 0+/-0.5, quadratic
	# these are put together to get the start time of each force span
	# rocket force is 200+/-100,  linear -- convert to MKS 0.2 +/- 0.1
	# rocket burn time is 0.25 +/- 0.1 linear
    force_span_list = []
    for stage in stage_rockets.keys() :
        for i in range(0,stage_rockets[stage]) :
            start_time = stage_start_time[stage] + frand_range( 0.0, 0.5, 2 ) # add the fuse length
            force      = frand_range( 0.400, 0.100, 1 )# force in newtons -- or it used to be ...
            burn_time  = frand_range( 0.25 , 0.1  , 2 ) # burn time in seconds
            # append that to the force span list
            fs = ForceSpan( t0 = start_time,
                            t1 = start_time+burn_time,
                            f1 = force,
                            f0 = 0 )
            force_span_list = insert_forcespan( force_span_list, fs )

    # now we can forward integrate to get the location span list
    location_span_list = forward_integrate( force_span_list, mass, friction )

    # and turn the location list into an event list
    # to do that we need a distance before the start of the track
    # and some arbitrary network clock
    packet_event_list = build_packet_event_list(
        race_track = race_track,
        location_span_list = location_span_list,
        distance_offset = Physical.convert( frand_range( 5,4,1),'in'),
        initial_network_clock = irand_range( (1<<32) , 1 ) # for testing the newtork clock
        )

    # with that we can build an 'extra' string for the description field
    extra = "{} rocket{} in {} stage{}".format(
        num_rockets,("s" if num_rockets!=1 else "") ,
        num_stages, ("s" if num_stages!=1 else "") )
    if num_stages > 1 :
        for stage in stage_rockets.keys() :
            extra += "{}{} rocket{} in stage {}".format(
                ( ", " if stage else " (" ),
                stage_rockets[stage],("s" if stage_rockets[stage] != 1 else ""),
                stage+1 )
        extra += ')'

    # return a PassData record

    return PassData.PassData (
        timestamp = None, # to indicate that we are fake ? datetime.datetime.now().timestamp() ,
        driver_name = "DevTeam{}".format( irand_range(  4,1 ) ) ,
        car_name = "FakeCar{}".format( irand_range( 10,1 ) ) ,
        car_mass = mass ,
        description = extra ,
        track_name = race_track.name ,
        packet_event_list = packet_event_list
        )






#   # testing
#
#   print()
#   print()
#   print("fake car")
#   print( create_fake_car() )
#
#   print()
#   print()
#   print("fake car")
#   print( create_fake_car() )
#
#   print()
#   print()
#   print("fake car")
#   print( create_fake_car() )




## ################################################################################################################################
## ################################################################################################################################
## ################################################################################################################################


## ok, now we've got an event list and collection of packets to send
## how do we go about doing that ??

#import datetime
#def get_packet() :
#
#    time_beg = datetime.datetime.now()
#    fake_car = create_fake_car()
#    for ev in fake_car.event_list :
#        while (datetime.datetime.now() - time_beg).total_seconds() < ev.t :
#            yield None
#        yield ev.pkt
#
# user would something like this ??
##for pkt in FakeCar.get_packet() :
##    if pkt is not None :
##        process_packet()
##    # otherwise sleep ?
